
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass.h
  * @author     baiyang
  * @date       2021-11-24
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <rtconfig.h>

#include "CompassCalibrator.h"
#include "sensor_compass_backend.h"
#include "sensor_compass_learn.h"

#include <mavproxy/mavproxy.h>

#include <common/gp_config.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
// motor compensation types (for use with motor_comp_enabled)
#define AP_COMPASS_MOT_COMP_DISABLED    0x00
#define AP_COMPASS_MOT_COMP_THROTTLE    0x01
#define AP_COMPASS_MOT_COMP_CURRENT     0x02
#define AP_COMPASS_MOT_COMP_PER_MOTOR   0x03

// setup default mag orientation for some board types
#ifndef MAG_BOARD_ORIENTATION
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
# define MAG_BOARD_ORIENTATION ROTATION_YAW_90
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI)
# define MAG_BOARD_ORIENTATION ROTATION_YAW_270
#else
# define MAG_BOARD_ORIENTATION ROTATION_NONE
#endif
#endif

#ifndef COMPASS_CAL_ENABLED
#if !defined(HAL_BUILD_AP_PERIPH)
#define COMPASS_CAL_ENABLED 1
#else
#define COMPASS_CAL_ENABLED 0
#endif
#endif

#if !defined(HAL_BUILD_AP_PERIPH)
#define COMPASS_MOT_ENABLED 
#endif

#if !defined(HAL_BUILD_AP_PERIPH)
#define COMPASS_LEARN_ENABLED
#endif

// define default compass calibration fitness and consistency checks
#define AP_COMPASS_CALIBRATION_FITNESS_DEFAULT 16.0f
#define AP_COMPASS_MAX_XYZ_ANG_DIFF radians(90.0f)
#define AP_COMPASS_MAX_XY_ANG_DIFF radians(60.0f)
#define AP_COMPASS_MAX_XY_LENGTH_DIFF 200.0f

/**
   maximum number of compass instances available on this platform. If more
   than 1 then redundant sensors may be available
 */
#ifndef HAL_COMPASS_MAX_SENSORS
#define HAL_COMPASS_MAX_SENSORS 3
#endif

#if HAL_COMPASS_MAX_SENSORS > 1
#define COMPASS_MAX_UNREG_DEV 5
#else
#define COMPASS_MAX_UNREG_DEV 0
#endif

#define COMPASS_MAX_INSTANCES HAL_COMPASS_MAX_SENSORS
#define COMPASS_MAX_BACKEND   HAL_COMPASS_MAX_SENSORS

#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)

/*----------------------------------typedef-----------------------------------*/
typedef uint8_t CompassPriority;
typedef uint8_t CompassStateIndex;

// bitmask of options
enum CompassOption {
    CAL_REQUIRE_GPS = (1U<<0),
};

enum CompassLearnType {
    LEARN_NONE=0,
    LEARN_INTERNAL=1,
    LEARN_EKF=2,
    LEARN_INFLIGHT=3
};

// enum of drivers for COMPASS_TYPEMASK
enum DriverType {
    COMPASS_DRIVER_HMC5843  =0,
    COMPASS_DRIVER_LSM303D  =1,
    COMPASS_DRIVER_AK8963   =2,
    COMPASS_DRIVER_BMM150   =3,
    COMPASS_DRIVER_LSM9DS1  =4,
    COMPASS_DRIVER_LIS3MDL  =5,
    COMPASS_DRIVER_AK09916  =6,
    COMPASS_DRIVER_IST8310  =7,
    COMPASS_DRIVER_ICM20948 =8,
    COMPASS_DRIVER_MMC3416  =9,
    COMPASS_DRIVER_UAVCAN   =11,
    COMPASS_DRIVER_QMC5883L =12,
    COMPASS_DRIVER_SITL     =13,
    COMPASS_DRIVER_MAG3110  =14,
    COMPASS_DRIVER_IST8308  =15,
    COMPASS_DRIVER_RM3100   =16,
    COMPASS_DRIVER_MSP      =17,
    COMPASS_DRIVER_SERIAL   =18,
    COMPASS_DRIVER_MMC5XX3  =19,
};

typedef struct {
    Param_int8  external;
    bool        healthy;
    bool        registered;
    CompassPriority priority;
    Param_int8     orientation;
    Param_Vector3f offset;
    Param_Vector3f diagonals;
    Param_Vector3f offdiagonals;
    Param_float    scale_factor;

    // device id detected at init.
    // saved to eeprom when offsets are saved allowing ram &
    // eeprom values to be compared as consistency check
    Param_int32    dev_id;
    // Initialised when compass is detected
    int32_t detected_dev_id;
    // Initialised at boot from saved devid
    int32_t expected_dev_id;

    // factors multiplied by throttle and added to compass outputs
    Param_Vector3f motor_compensation;

    // latest compensation added to compass
    Vector3f_t    motor_offset;

    // corrected magnetic field strength
    Vector3f_t    field;

    // when we last got data
    uint32_t    last_update_ms;
    uint64_t    last_update_usec;

    // board specific orientation
    enum RotationEnum rotation;

    // accumulated samples, protected by _sem, used by AP_Compass_Backend
    Vector3f_t accum;
    uint32_t accum_count;
}mag_state;

typedef struct {
    //Create an Array of mag_state to be accessible by StateIndex only
    mag_state _state[COMPASS_MAX_INSTANCES+1];

    //Create Arrays to be accessible by Priority only
    Param_int8 _use_for_yaw[COMPASS_MAX_INSTANCES];

#if COMPASS_MAX_INSTANCES > 1
    Param_int32 _priority_did_stored_list[COMPASS_MAX_INSTANCES];
    int32_t _priority_did_list[COMPASS_MAX_INSTANCES];
#endif

    CompassPriority Priority;
    CompassStateIndex StateIndex;

    // board orientation from AHRS
    enum RotationEnum _board_orientation;

    // custom board rotation matrix
    matrix3f_t *_custom_rotation;

    // custom external compass rotation matrix
    matrix3f_t *_custom_external_rotation;

    // whether to enable the compass drivers at all
    Param_int8     _enabled;
    bool           init_done;

    Param_int8 _filter_range;

    // declination in radians
    Param_float    _declination;

    // enable automatic declination code
    Param_int8     _auto_declination;

    // stores which bit is used to indicate we should log compass readings
    uint32_t       _log_bit;

    // motor compensation type
    // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
    Param_int8     _motor_comp_type;

    // automatic compass orientation on calibration
    Param_int8     _rotate_auto;

    // custom compass rotation
    Param_float    _custom_roll;
    Param_float    _custom_pitch;
    Param_float    _custom_yaw;
    
    // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
    float          _thr;

    Param_int16    _offset_max;
    Param_int16    _options;

    Param_float    _calibration_threshold;

    // mask of driver types to not load. Bit positions match DEVTYPE_ in backend
    Param_int32    _driver_type_mask;

#if COMPASS_MAX_UNREG_DEV
    // Put extra dev ids detected
    Param_int32 extra_dev_id[COMPASS_MAX_UNREG_DEV];
    uint32_t    _previously_unreg_mag[COMPASS_MAX_UNREG_DEV];
#endif

    // backend objects
    sensor_compass_backend *_backends[COMPASS_MAX_BACKEND];
    uint8_t     _backend_count;

    // number of registered compasses.
    uint8_t     _compass_count;

    // number of unregistered compasses.
    uint8_t     _unreg_compass_count;

    // settable parameters
    Param_int8 _learn;

    bool _initial_location_set;
    bool _cal_thread_started;

    bool learn_allocated;

    uint8_t _first_usable; // first compass usable based on COMPASSx_USE param

    CompassCalibrator* _calibrator[COMPASS_MAX_INSTANCES];

    //keep track of which calibrators have been saved
    bool _cal_saved[COMPASS_MAX_INSTANCES];
    bool _cal_autosave;

    //autoreboot after compass calibration
    bool _compass_cal_autoreboot;
    bool _cal_requires_reboot;
    bool _cal_has_run;

    CompassPriority next_cal_progress_idx[MAVLINK_COMM_NUM_BUFFERS];
    CompassPriority next_cal_report_idx[MAVLINK_COMM_NUM_BUFFERS];
} sensor_compass;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
sensor_compass *sensor_compass_get_singleton();

// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
void sensor_compass_ctor(void);
void sensor_compass_init();
bool sensor_compass_read(void);
bool sensor_compass_have_scale_factor(uint8_t i);
bool sensor_compass_register_compass(int32_t dev_id, uint8_t* instance);
/// Return the health of a compass
bool sensor_compass_healthy2(uint8_t i);
bool sensor_compass_healthy(void);
bool sensor_compass_available();
bool sensor_compass_use_for_yaw(void);
bool sensor_compass_use_for_yaw2(uint8_t i);
uint64_t sensor_compass_last_update_usec(void);
uint64_t sensor_compass_last_update_usec2(uint8_t i);
uint32_t sensor_compass_last_update_ms(void);
uint32_t sensor_compass_last_update_ms2(uint8_t i);
const Vector3f_t *sensor_compass_get_field2(uint8_t i);
const Vector3f_t *sensor_compass_get_field(void);
CompassPriority sensor_compass_get_priority(CompassStateIndex state_id);

void sensor_compass_assign_param();
void sensor_compass_priority_did_stored_save_param(uint8_t index, int32_t val);
void sensor_compass_extra_dev_id_save_param(uint8_t index, int32_t val);
void sensor_compass_extra_dev_id_save_param2(uint8_t index);
void sensor_compass_extra_dev_id_set_param(uint8_t index, int32_t val);
void sensor_compass_dev_id_set(uint8_t index, int32_t val);
void sensor_compass_dev_id_save(uint8_t index);
void sensor_compass_dev_id_set_and_save(uint8_t index, int32_t val);
void sensor_compass_offset_save(uint8_t index);
void sensor_compass_mag_state_copy_from(mag_state* state_des, uint8_t index, const mag_state* state_src);

const mag_state* sensor_compass_get_state(CompassPriority priority);

/// Sets offset x/y/z values.
///
/// @param  i                   compass instance
/// @param  offsets             Offsets to the raw mag_ values in milligauss.
///
void sensor_compass_set_offsets(uint8_t i, const Vector3f_t* offsets);

/// Sets and saves the compass offset x/y/z values.
///
/// @param  i                   compass instance
/// @param  offsets             Offsets to the raw mag_ values in milligauss.
///
void sensor_compass_set_and_save_offsets(uint8_t i, const Vector3f_t* offsets);
void sensor_compass_set_and_save_diagonals(uint8_t i, const Vector3f_t* diagonals);
void sensor_compass_set_and_save_offdiagonals(uint8_t i, const Vector3f_t* diagonals);
void sensor_compass_set_and_save_scale_factor(uint8_t i, float scale_factor);
void sensor_compass_set_and_save_orientation(uint8_t i, enum RotationEnum orientation);

/// Saves the current offset x/y/z values for one or all compasses
///
/// @param  i                   compass instance
///
/// This should be invoked periodically to save the offset values maintained by
/// ::learn_offsets.
///
void sensor_compass_save_offsets2(uint8_t i);
void sensor_compass_save_offsets(void);

void sensor_compass_reset_compass_id();

/// Compass Calibrator
void sensor_compass_cal_init();
void sensor_compass_cal_update();
bool sensor_compass_is_calibrating();
MAV_RESULT sensor_compass_handle_mag_cal_command(const mavlink_command_long_t* packet);
bool sensor_compass_send_mag_cal_progress(mavlink_message_t *msg_t);
bool sensor_compass_send_mag_cal_report(mavlink_message_t *msg_t);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



